/*
 * Messaging.cpp
 *
 *  Created on: 19.11.2012
 *      Author: Michael
 */

#include "rodos.h"
#include "TTC.h"

#include "hal_uart.h"
#include "stm32f4xx_usart.h"
#include "stm32f4xx.h"

#include <stdio.h>
#include <stdlib.h>

#include "../hardware.h"

#include "global.h"

#include "../Messaging/TelemetryFrame.h"
#include "../Messaging/protocol.h"
#include "Calibrate.h"
#include "../PIDController/AnglePID.h"
#include "../PIDController/VelocityPID.h"
#include "../SMControl/AngleSMC.h"
#include "../SMControl/VelocitySMC.h"
extern Systemstate state;

extern PowerDriver motor;
extern Camera cam;

extern Magnetometer mag;

extern ADConverter adc;

extern AnglePID anglePID;
extern VelocityPID velPID;
extern AngleSMC angleSMC;
extern VelocitySMC velSMC;

TTC::TTC(const char* name, const long prio) :
		Thread(name, prio) {

	// DMA for UART2
	RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_DMA1, ENABLE);

	DMA_InitTypeDef DMA_InitStruct;
	DMA_StructInit(&DMA_InitStruct);

	DMA_InitStruct.DMA_Channel = DMA_Channel_4;
	DMA_InitStruct.DMA_PeripheralBaseAddr = (uint32_t) &(USART2 ->DR);
	DMA_InitStruct.DMA_Memory0BaseAddr = (uint32_t) cam.DCMI_Buffer;
	DMA_InitStruct.DMA_DIR = DMA_DIR_MemoryToPeripheral;
	DMA_InitStruct.DMA_BufferSize = IMAGEBUFFERSIZE;
	DMA_InitStruct.DMA_PeripheralInc = DMA_PeripheralInc_Disable;
	DMA_InitStruct.DMA_MemoryInc = DMA_MemoryInc_Enable;
	DMA_InitStruct.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte;
	DMA_InitStruct.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte;
	DMA_InitStruct.DMA_Mode = DMA_Mode_Normal;
	DMA_InitStruct.DMA_Priority = DMA_Priority_High;
	DMA_InitStruct.DMA_FIFOMode = DMA_FIFOMode_Disable;
	DMA_InitStruct.DMA_FIFOThreshold = DMA_FIFOThreshold_HalfFull;
	DMA_InitStruct.DMA_MemoryBurst = DMA_MemoryBurst_Single;
	DMA_InitStruct.DMA_PeripheralBurst = DMA_PeripheralBurst_Single;
	DMA_Init(DMA1_Stream6, &DMA_InitStruct);

	NVIC_InitTypeDef NVIC_InitStructure;
	NVIC_InitStructure.NVIC_IRQChannel = DMA1_Stream6_IRQn;
	NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;
	NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
	NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
	NVIC_Init(&NVIC_InitStructure);

}

TTC::~TTC() {

}

// UART2 - Rx:PD6  Tx:PD5
void TTC::init() {
//	uart2.init(BAUDRATE);
//	uart2.init(256000);
}

void TTC::run() {
	setPeriodicBeat(2105 * MILLISECONDS, TELEMERTY_TIME * MILLISECONDS);
	int cnt = sprintf(buffer, "TTC running\n");
	uart2.write(buffer, cnt);
	sendChar(0x0A);

	suspendUntilNextBeat();
	cnt = sprintf(buffer, "GyroBias = %f\n", state.gyroBias);
	uart2.write(buffer, cnt);
	while (1) {
#ifndef CAL_MAG
		if (state.frameReceived) {
			state.frameReceived = false;
			sendPayload();
			BLUE_OFF;
//			suspendCallerUntil(NOW() + 2*SECONDS);		// suspend until payload is sent
		} else {
			sendTelemetry();
			receiveCommand();
		}
		suspendUntilNextBeat();
#endif
	}
}

void TTC::receiveCommand() {
	int readCnt = uart2.read(buffer, BUFFERSIZE);

	msgTypes msgType;
	int floatRec = 0;

	if (readCnt) {
		parseMsg(buffer, readCnt, &floatRec, &msgType);
		if (msgType == SET_PWM) {
//			state.dutycycle = values[0];

			state.dutycycle = values[0];
			motor.setSpeedPercentage(state.dutycycle);
		} else if (msgType == GET_PID_VEL) {
			TelemetryFrame f(GET_PID_VEL);
			f.addValue(state.k_pVel);
			f.addValue(state.k_iVel);
			f.addValue(state.k_dVel);
			f.endFrame();
			uart2.write(f.buffer, f.length);
		} else if (msgType == GET_PID_ANGLE) {
			TelemetryFrame f(GET_PID_ANGLE);
			f.addValue(state.k_pAngle);
			f.addValue(state.k_iAngle);
			f.addValue(state.k_dAngle);
			f.endFrame();
			uart2.write(f.buffer, f.length);
		} else if (msgType == SET_PID_ANGLE) {
			state.k_pAngle = values[0];
			state.k_iAngle = values[1];
			state.k_dAngle = values[2];

			anglePID.resetPID();

		} else if (msgType == SET_PID_VEL) {
			state.k_pVel = values[0];
			state.k_iVel = values[1];
			state.k_dVel = values[2];

			velPID.resetPID();

		} else if (msgType == SET_KALMAN) {
			state.Q_angle = values[0];
			state.magVar = values[1];
			state.gyroBias = values[2];
		} else if (msgType == GET_KALMAN) {
			TelemetryFrame f(GET_KALMAN);
			f.addValue(state.Q_angle);
			f.addValue(state.magVar);
			f.addValue(state.gyroBias);
			f.endFrame();
			uart2.write(f.buffer, f.length);
		} else if(msgType == SET_SMC_ANGLE){
			state.Gain2ang = values[0];
			state.Gain3ang = values[1];
			state.Gain4ang = values[2];
		} else if(msgType == GET_SMC_ANGLE){
			TelemetryFrame f(GET_SMC_ANGLE);
			f.addValue(state.Gain2ang);
			f.addValue(state.Gain3ang);
			f.addValue(state.Gain4ang);
			f.endFrame();
			uart2.write(f.buffer, f.length);
		} else if(msgType == SET_SMC_VEL){
			state.Gain2Vel = values[0];
			state.Gain3Vel = values[1];
			state.Gain4Vel = values[2];
		} else if(msgType == GET_SMC_VEL){
			TelemetryFrame f(GET_SMC_VEL);
			f.addValue(state.Gain2Vel);
			f.addValue(state.Gain3Vel);
			f.addValue(state.Gain4Vel);
			f.endFrame();
			uart2.write(f.buffer, f.length);
		} else if (msgType == NONE) {
//			state.PID_Angle = true;
			state.PID_Angle = false;
			state.PID_Vel = false;
			state.SMC_Angle = false;
			state.SMC_Vel = false;
			motor.setSpeedPercentage(0);
//			state.desAngle = 0;
		} else if (msgType == TAKE_PICTURE_AT) {
			state.notTaken = true;
			state.PID_Angle = true;
			state.PID_Vel = false;
			state.SMC_Angle = false;
			state.SMC_Vel = false;
			state.desAngle = values[0];

			anglePID.resetPID();

		} else if (msgType == TURN_W_VELOCITY) {
			state.PID_Angle = false;
			state.PID_Vel = true;
			state.SMC_Angle = false;
			state.SMC_Vel = false;
			state.desVel = values[0];

			velPID.resetPID();

		} else if (msgType == TAKE_PICTURE_AT_SMC) {
			state.notTaken = true;
			state.PID_Angle = false;
			state.PID_Vel = false;
			state.SMC_Angle = true;
			state.SMC_Vel = false;
			state.desAngle = values[0];

			angleSMC.resetSMC();
		} else if (msgType == TURN_W_VELOCITY_SMC) {
			state.PID_Angle = false;
			state.PID_Vel = false;
			state.SMC_Angle = false;
			state.SMC_Vel = true;
			state.desVel = values[0];

			velSMC.resetSMC();
		} else if (msgType == SNAPSHOT) {
			cam.Capture();
			xprintf("Take Snapshot\n");
		} else if (msgType == SAT_RESET) {
			ORANGE_OFF;
//			cal.run();
			mag.setFirstrun(true);
			motor.setSpeedPercentage(0);
			state.PID_Angle = false;
			state.PID_Vel = false;
			state.SMC_Angle = false;
			state.SMC_Vel = false;
			state.desAngle = 0;
			state.desVel = 0;

		}
	}
}

void TTC::sendTelemetry() {
	TelemetryFrame f(TELEMETRY);
	f.addValue(state.angleKalman);
	f.addValue(state.angleGyro);
	f.addValue(state.angleMag);

	f.addValue(state.velKalman);
	f.addValue(state.velGyro - state.gyroBias);
	f.addValue(state.velMag);

	f.addValue(state.temperature);

	f.addValue(adc.getBatteryVoltage());
//	f.addValue(0);


	// PID values
	f.addValue(anglePID.P);
	f.addValue(anglePID.I);
	f.addValue(anglePID.D);

	f.addValue(velPID.P);
	f.addValue(velPID.I);
	f.addValue(velPID.D);

	f.addValue(state.dutycycle);

	f.endFrame();
	uart2.write(f.buffer, f.length);
}


void TTC::sendPayload() {

	xprintf("SEND\n");
	sendChar(0x0A);

	cam.DisableDCMI();
	cam.EnableDCMI();
	DMA_ITConfig(DMA1_Stream6, DMA_IT_TC, ENABLE);
	USART_DMACmd(USART2, USART_DMAReq_Tx, ENABLE);
	DMA_Cmd(DMA1_Stream6, ENABLE);
}

float TTC::Char2Float(char* buf) {
	uint32_t raw_data;
	raw_data = buf[0] << 24 | buf[1] << 16 | buf[2] << 8 | buf[3];
	float* out = (float*) ((void*) &raw_data);
	return *out;
}

int TTC::parseMsg(char* buffer, int readCnt, int* floatRec, msgTypes* msgType) {

	int receiverState = DEFAULT;
	int frameLength = 0;

	for (int i = 0; i < readCnt; i++) {
		switch (buffer[i]) {
		case FRAMESTART1:
			if (receiverState == DEFAULT)
				receiverState = START1;
			break;
		case FRAMESTART2:
			if (receiverState == START1)
				receiverState = START2;
			break;
		case FRAMESTART3:
			if (receiverState == START2)
				receiverState = START3;
			break;
		case FRAMESTART4:
			if (receiverState == START3) {
				receiverState = START4;
				frameLength = (buffer[i + 1] << 8 | buffer[i + 2]);
				xprintf("Length = %d", frameLength);
				*msgType = (msgTypes) buffer[i + 3];
				xprintf(" Type = %d ", (int) *msgType);
				i += 3;
				if (frameLength != readCnt) {
					return -2;
				}
			}
			break;
		case HEADSEPERATOR:
		case VALSEPERATOR:
			if (receiverState == START4) {
				values[*floatRec] = Char2Float((char*) (buffer + i + 1));
				xprintf("%f ", values[*floatRec]);
				(*floatRec)++;
				i += 4;
			}
			break;
		case FRAMEEND:
			if (receiverState == START4) {
				receiverState = END;
				xprintf(" recValues =  %d ", *floatRec);
				xprintf("\n");
				return 0;
			}
			for (int i = 0; i < BUFFERSIZE; i++) {
				buffer[i] = ' ';
			}
			break;
		default:
			break;
		}
	}
	return -1;
}

void TTC::sendChar(char ch) {
	uint8_t c = ch;
	USART_SendData(USART2, c);
	/* Loop until the end of transmission */
	while (USART_GetFlagStatus(USART2, USART_FLAG_TC ) == RESET) {
	}
}

